//
// Created by luozj on 3/4/24.
//

#ifndef _KINEMATICS_H
#define _KINEMATICS_H
#include "modern_robotics.h"
namespace qy{
    struct DH{
        double alpha;
        double a;
        double d;
        double theta;
        double offset;
        DH(){
            alpha=a=d=theta=offset=0;
        }
        DH(double alpha_,double a_,double d_,double theta_,double offset_){
            alpha=alpha_;
            a=a_;
            d=d_;
            theta=theta_;
            offset=offset_;
        }
    };

    Eigen::Matrix4d dhFormMat(const DH &dh);

    Eigen::MatrixXd FKinSpaceDH(const Eigen::VectorXd &thetalist);
}
#endif //_KINEMATICS_H
